import cv2
import mediapipe as mp
from utils.config_loader import cfg

class PoseDetector:
    def __init__(self):
        # 加载配置
        self.mode = cfg.getboolean('PoseDetection', 'model')
        self.model_complexity = cfg.getint('PoseDetection', 'model_complexity')
        self.smooth_landmarks = cfg.getboolean('PoseDetection', 'smooth_landmarks')
        self.enable_segmentation = cfg.getboolean('PoseDetection', 'enable_segmentation')
        self.smooth_segmentation = cfg.getboolean('PoseDetection', 'smooth_segmentation')
        self.min_detection_confidence = cfg.getfloat('PoseDetection', 'min_detection_confidence')
        self.min_tracking_confidence = cfg.getfloat('PoseDetection', 'min_tracking_confidence')

        # 加载动作阈值
        self.punch_threshold = cfg.getfloat('GameActions', 'punch_threshold')
        self.kick_threshold = cfg.getfloat('GameActions', 'kick_threshold')
        self.forward_threshold = cfg.getfloat('GameActions', 'forward_threshold')
        self.squat_threshold = cfg.getfloat('GameActions', 'squat_threshold')
        self.jump_threshold = cfg.getfloat('GameActions', 'jump_threshold')
        self.kamehameha_threshold = cfg.getfloat('GameActions', 'kamehameha_threshold')
        self.initial_height_ratio = None  # 初始高度比例
        self.height_ratio_threshold = 0.15  # 高度变化阈值

        # 初始化MediaPipe，创建pose用于检测frame、创建draw用于在frame上标注内容
        self.mp_pose = mp.solutions.pose
        self.pose = self.mp_pose.Pose(self.mode, self.model_complexity,
                                    self.smooth_landmarks, self.enable_segmentation,
                                    self.smooth_segmentation, self.min_detection_confidence,
                                    self.min_tracking_confidence)
        self.mp_drawing = mp.solutions.drawing_utils

        self.landmarks = None
    
    # 检测动作，并放置结果
    def find_pose(self, img, draw=True):
        # print(f"[DEBUG] Processing frame of size {img.shape}")
        img_rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
        self.results = self.pose.process(img_rgb)
        
        # Initialize default return value
        punch_res = {}
        kick_res = {}
        pose_results={}
        forward_res={}
        squat_res={}
        jump_res={}
        kamehameha_res={}
        
        if self.results.pose_landmarks:
            # print("[DEBUG] Pose landmarks detected")
            if draw:
                self.mp_drawing.draw_landmarks(img, self.results.pose_landmarks,
                                             self.mp_pose.POSE_CONNECTIONS)
            
            # Get landmarks
            landmarks = []
            for id, lm in enumerate(self.results.pose_landmarks.landmark):
                h, w, c = img.shape
                cx, cy = int(lm.x * w), int(lm.y * h)
                landmarks.append([id, cx, cy])
            self.landmarks = landmarks
            
            # 出拳调试版
            punch_res = self.check_punch_debug(img,landmarks)
            # 踢调试版
            kick_res = self.check_kick_debug(img, landmarks)
            # 前进调试版
            forward_res = self.check_forward_debug(img, landmarks)
            # 深蹲调试版
            squat_res = self.check_squat_debug(img, landmarks)
            # 跳跃调试版
            jump_res = self.check_jump_debug(img, landmarks)
            # 龟派气功调试版
            kamehameha_res = self.check_kamehameha_debug(img, landmarks)
            
            pose_results = {**punch_res,**kick_res, **forward_res,**squat_res,**jump_res,**kamehameha_res}
            
        return pose_results

    # 出拳调试版
    def check_punch_debug(self, img, landmarks):
        """
        返回结果同上，但会在 img 上画调试信息，方便肉眼调阈值
        """
        h, w, _ = img.shape

        # 取关键点
        left_wrist = next(((x[1], x[2]) for x in landmarks if x[0] == 15), None)
        left_shoulder = next(((x[1], x[2]) for x in landmarks if x[0] == 11), None)
        right_wrist = next(((x[1], x[2]) for x in landmarks if x[0] == 16), None)
        right_shoulder = next(((x[1], x[2]) for x in landmarks if x[0] == 12), None)

        result = {'is_punch': False, 'hand': None}

        def _test_one_side(wrist, shoulder, hand: str):
            nonlocal result
            if not wrist or not shoulder:
                return
            # 距离
            dist = ((wrist[0] - shoulder[0]) ** 2 + (wrist[1] - shoulder[1]) ** 2) ** 0.5
            # 画连线
            cv2.line(img, shoulder, wrist, (255, 255, 0), 2)
            # 写距离
            mid = (shoulder[0] + wrist[0]) // 2, (shoulder[1] + wrist[1]) // 2
            cv2.putText(img, f'dist {dist:.1f}', mid, cv2.FONT_HERSHEY_SIMPLEX,
                        0.6, (255, 255, 255), 2)
            # 阈值参考圆（肩为中心）
            thr = self.punch_threshold * 100
            cv2.circle(img, shoulder, int(thr), (0, 255, 0), 1)

            if dist < thr:
                result.update({'is_punch': True, 'hand': hand})
                # 触发时把连线变红
                cv2.line(img, shoulder, wrist, (0, 0, 255), 3)
                cv2.putText(img, f'{hand} PUNCH!', (shoulder[0], shoulder[1] - 20),
                            cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)

        _test_one_side(left_wrist, left_shoulder, 'left')
        _test_one_side(right_wrist, right_shoulder, 'right')

        return result
    # 踢调试版
    def check_kick_debug(self, img, landmarks):
        """
        膝→踝半径检测：画膝-踝连线、实时距离、阈值圆（以膝为中心）
        """
        h, w, _ = img.shape

        # 取关键点
        left_knee = next(((x[1], x[2]) for x in landmarks if x[0] == 25), None)
        left_ankle = next(((x[1], x[2]) for x in landmarks if x[0] == 27), None)
        right_knee = next(((x[1], x[2]) for x in landmarks if x[0] == 26), None)
        right_ankle = next(((x[1], x[2]) for x in landmarks if x[0] == 28), None)

        result = {'is_kick': False, 'leg': None}

        def _test_one_side(knee, ankle, leg: str):
            nonlocal result
            if not knee or not ankle:
                return
            # 膝→踝距离
            dist = ((ankle[0] - knee[0]) ** 2 + (ankle[1] - knee[1]) ** 2) ** 0.5
            # 连线
            cv2.line(img, knee, ankle, (255, 255, 0), 2)
            # 写距离
            mid = (knee[0] + ankle[0]) // 2, (knee[1] + ankle[1]) // 2
            cv2.putText(img, f'{dist:.1f}', mid,
                        cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)
            # 阈值参考圆（绿色，以膝盖为中心）
            thr = self.kick_threshold * 100  # 同样先用像素单位
            cv2.circle(img, knee, int(thr), (0, 255, 0), 1)

            if dist < thr:
                result.update({'is_kick': True, 'leg': leg})
                # 触发：线变红 + 大字
                cv2.line(img, knee, ankle, (0, 0, 255), 3)
                cv2.putText(img, f'{leg.upper()} KICK!', (knee[0], knee[1] - 20),
                            cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)

        _test_one_side(left_knee, left_ankle, 'left')
        _test_one_side(right_knee, right_ankle, 'right')

        return result
    # 前进调试版
    def check_forward_debug(self, img, landmarks):
        """
        前进检测：髋中心→脚踝水平距离 > 阈值 视为前跨一步
        """
        h, w, _ = img.shape

        # 取髋部左右点
        left_hip = next(((x[1], x[2]) for x in landmarks if x[0] == 23), None)
        right_hip = next(((x[1], x[2]) for x in landmarks if x[0] == 24), None)
        # 取脚踝
        left_ankle = next(((x[1], x[2]) for x in landmarks if x[0] == 27), None)
        right_ankle = next(((x[1], x[2]) for x in landmarks if x[0] == 28), None)

        result = {'is_forward': False, 'leg': None}

        if not (left_hip and right_hip and left_ankle and right_ankle):
            return result

        # 髋中心
        hip_cx = (left_hip[0] + right_hip[0]) // 2
        hip_cy = (left_hip[1] + right_hip[1]) // 2
        hip_center = (hip_cx, hip_cy)

        def _test_one_side(ankle, leg: str):
            nonlocal result
            # 只用水平距离（x 方向）
            # print('ankle',ankle[0])
            # print('hip_center',hip_center[0])
            dx = ankle[0] - hip_center[0]
            dist = abs(dx)
            # 画垂直参考线（髋中心→踝 y 同高）
            cv2.line(img, hip_center, (ankle[0], hip_center[1]), (255, 255, 0), 2)
            # 写水平距离
            mid = (hip_center[0] + ankle[0]) // 2, hip_center[1] - 20
            cv2.putText(img, f'forward {dist:.1f}', mid,
                        cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)
            # 阈值参考圆（绿色）
            thr = self.forward_threshold * 100  # 同比例缩放
            cv2.circle(img, hip_center, int(thr), (0, 255, 0), 1)

            if dist > thr:
                print(f'dist {dist:.1f}')
                print(f'thr {thr:.1f}')
                result.update({'is_forward': True, 'leg': leg})
                # 触发：线变红 + 大字dddd
                cv2.line(img, hip_center, (ankle[0], hip_center[1]), (0, 0, 255), 3)
                cv2.putText(img, 'FORWARD!', (hip_center[0], hip_center[1] - 40),
                            cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 0, 255), 3)

        # 分别检测左右脚
        _test_one_side(left_ankle, 'left')
        _test_one_side(right_ankle, 'right')

        return result
    # 下蹲调试版
    def check_squat_debug(self, img, landmarks):
        """
        下蹲检测：髋中心与膝盖的垂直距离 < 阈值 视为下蹲
        """
        h, w, _ = img.shape

        # 取髋部和膝盖关键点
        left_hip = next(((x[1], x[2]) for x in landmarks if x[0] == 23), None)
        right_hip = next(((x[1], x[2]) for x in landmarks if x[0] == 24), None)
        left_knee = next(((x[1], x[2]) for x in landmarks if x[0] == 25), None)
        right_knee = next(((x[1], x[2]) for x in landmarks if x[0] == 26), None)

        result = {'is_squat': False}

        if not (left_hip and right_hip and left_knee and right_knee):
            return result

        # 髋中心
        hip_center = ((left_hip[0] + right_hip[0]) // 2, 
                      (left_hip[1] + right_hip[1]) // 2)
        # 膝中心
        knee_center = ((left_knee[0] + right_knee[0]) // 2,
                       (left_knee[1] + right_knee[1]) // 2)

        # 垂直距离
        dy = knee_center[1] - hip_center[1]
        dist = abs(dy)
        
        # 画参考线（髋中心→膝中心）
        cv2.line(img, hip_center, knee_center, (255, 255, 0), 2)
        # 写垂直距离
        cv2.putText(img, f'dist {dist:.1f}', (hip_center[0], hip_center[1] - 20),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)
        # 阈值参考线（绿色水平线）
        thr = self.squat_threshold * 100
        cv2.line(img, (hip_center[0]-50, hip_center[1]+int(thr)), 
                 (hip_center[0]+50, hip_center[1]+int(thr)), (0, 255, 0), 1)

        if dist < thr:
            result['is_squat'] = True
            # 触发：线变红 + 大字
            cv2.line(img, hip_center, knee_center, (0, 0, 255), 3)
            cv2.putText(img, 'SQUAT!', (hip_center[0], hip_center[1] - 40),
                        cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 0, 255), 3)

        return result
    # 跳跃调试版
    def check_jump_debug(self, img, landmarks):
        # 获取关键点
        left_hip = next(((x[1], x[2]) for x in landmarks if x[0] == 23), None)
        right_hip = next(((x[1], x[2]) for x in landmarks if x[0] == 24), None)
        left_ankle = next(((x[1], x[2]) for x in landmarks if x[0] == 27), None)
        right_ankle = next(((x[1], x[2]) for x in landmarks if x[0] == 28), None)
        
        if not (left_hip and right_hip and left_ankle and right_ankle):
            return {'is_jump': False, 'height_change': 0}
        
        # 计算髋部和脚踝平均高度
        hip_avg = (left_hip[1] + right_hip[1]) / 2
        ankle_avg = (left_ankle[1] + right_ankle[1]) / 2
        
        # 初始化基准高度（第一次检测时）
        if self.initial_height_ratio is None:
            self.initial_height_ratio = ankle_avg / hip_avg
        
        # 计算当前高度比例变化
        current_ratio = ankle_avg / hip_avg
        height_change = (self.initial_height_ratio - current_ratio) / self.initial_height_ratio
        
        # 判断跳跃条件
        is_jump = height_change > self.height_ratio_threshold
        
        # 绘制调试信息
        cv2.putText(img, f'Height Change: {height_change:.2%}', 
                (50, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2)
        
        if is_jump:
            cv2.putText(img, 'JUMP!', (img.shape[1]//2, 50),
                    cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 3)
        
        return {'is_jump': is_jump, 'height_change': height_change}

    def check_kamehameha_debug(self, img, landmarks):
        """
        检测龟派气功动作（双手合拢向前推）
        判断标准：
        1. 双手距离小于阈值
        2. 手臂角度符合向前推的动作
        3. 持续时长达到要求
        """
        # 获取关键点坐标
        left_wrist = next(((x[1], x[2]) for x in landmarks if x[0] == 15), None)
        right_wrist = next(((x[1], x[2]) for x in landmarks if x[0] == 16), None)
        
        if not (left_wrist and right_wrist):
            return {'is_kamehameha': False}
        
        # 计算双手距离
        distance = ((left_wrist[0] - right_wrist[0])**2 + 
                (left_wrist[1] - right_wrist[1])**2)**0.5
        
        # 绘制调试信息
        cv2.putText(img, f"Hands Distance: {distance:.2f}", 
                (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2)
        
        # 判断是否触发动作
        if distance < self.kamehameha_threshold:
            cv2.putText(img, "KAMEHAMEHA!", (img.shape[1]//2, 50), 
                    cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 3)
            return {'is_kamehameha': True}
        
        return {'is_kamehameha': False}